% implement tranditional SMC via ode45

params=struct();
params.time=0:0.01:10;
params.initial=[2 -1];
params.delta=rand(size(params.time));
params.k=1;
params.c=1;
[~,s0]=model(0,params.initial,params);

[t,y]=ode45(@(t,x)model(t,x,params),params.time,params.initial);
s=repmat(s0,size(t,1),1);
for it=1:length(t)
    [~,si]=model(t(it),y(it,:)',params);
    s(it,:)=si;
end


figure;tiledlayout(3,1)
nexttile;hold on;grid on;
plot(t,y(:,1));
plot(t,y(:,2));
plot(t,vertcat(s.s));
legend("$x_1$","$x_2$","$s=c x_1+x_2$","Interpreter","latex");
nexttile;hold on;grid on;
plot(t,vertcat(s.dxdt1))
plot(t,vertcat(s.dxdt2))
legend("$\dot{x}_1$","$\dot{x}_2$","Interpreter","latex");
nexttile;hold on;grid on;
plot(t,vertcat(s.u1));
plot(t,vertcat(s.u2));
legend("u1","u2","Interpreter","latex");


function [dydt,s]=model(t,y,p)
    x1=y(1);x2=y(2);
    s.delta=interp1(p.time,p.delta,t,"cubic");
    s.s=p.c*x1+x2;
    s.u1=-p.k*sat(10*s.s,1);
    s.u2=0;%-sat(p.c*x1,0.00003);
    s.dxdt1=x2;
    s.dxdt2=s.u1+s.u2+s.delta;
    dydt=[s.dxdt1;s.dxdt2];
end

function y=sat(x,a)
    y=max(min(x,a),-a);
end